/******************************************************************************
;	OS.c 
;	Jonathan Valvano, Feb 4, 2011, EE345M
;	Matthew Villarrubia, Yves Santos 2/28/2011
******************************************************************************/
#include<stdlib.h> 
#include "os.h"
#include "inc/hw_ints.h"
#include "driverlib/interrupt.h"
#include "utlist.h"
#include "timers.h"
#include "adc_c.h"
#define NVIC_ST_CURRENT_R       (*((volatile unsigned long *)0xE000E018))

volatile unsigned long long OSTime = 0;
volatile unsigned long long Global_timerMs = 0;

bool_t OSRunning = true;
OS_TCB *OSTCBCur; 									//TCBCur pointer to the current thread TCB
OS_TCB *TCBList[MAX_NUM_PRIORITIES+1][2] = {NULL};	//Array of linked lists. Col 0 is list head.
													//Col 1 is current thread for that priority.
													//Row# is priority

Sema4Type BoxFree;									//Semaphores for mailbox
Sema4Type DataValid;

unsigned long MailBoxData;
unsigned long SystemTimer = 0;

void SystemTime(void) {
	SystemTimer++;
}


//********************************OS_Init**************************************
//	Initializes the operating system. Sets up peripherals. Initializes systick.
//! \param None
//! \return None
//*****************************************************************************
void OS_Init(void) {
	SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_8MHZ);

	//InitConsole();
	//RIT128x96x4Init(1000000);
	//RIT128x96x4Clear();
	//InitADCTimerTrigger(1);
	//InitADC0(0);
	//InitADC_Processor(1); 
	SysTickInit(1);
	//OS_MailBox_Init();
}

//****************************OS_InitSemaphore**********************************
//	Initializes a semaphore.
//! \param semaPt Pointer to the semaphore to initialize
//! \param value Value to initialize the semaphore to. This paramater should
//!	almost always be 0.
//!	\param maxNumThreads For counting semaphores, this should be a number
//!	greater than 1. For binary semaphores, this should equal 1.
//! \return None
//! TODO Eliminate the value field. It is not really used
//*****************************************************************************
void OS_InitSemaphore(Sema4Type *semaPt, long value, long maxNumThreads){
	semaPt->Value = value;
	semaPt->MaxNumThreads = maxNumThreads; //for counting sema4s
} 

//*********************************OS_Wait**************************************
//	Spin / block on a counting semaphore.
//! \param *thread Pointer to the thread that is calling this function
//! \param semaPt Pointer to the semaphore to wait on.
//! \return None
//*****************************************************************************
void OS_Wait(OS_TCB *thread, Sema4Type *semaPt){
	while(1){
		IntMasterDisable();//!TODO - only disable timer interrupt for scheduler
	   	if(semaPt->Value < semaPt->MaxNumThreads){
			semaPt->Value++;
			IntMasterEnable();//!TODO - only enable timer interrupt for scheduler
			break;
		}else{
			thread->oldPriority = thread->priority; //Preserve the original priority for
													//when the thread is "woken up"
			thread->sema4BlockPt = semaPt;			//If the semaphore is occupied, then
			OS_SetPriority(thread,BLOCKED);			//move the thread to the blocked list
			IntMasterEnable();						//and trigger a context switch
			OS_Suspend();
		}
		IntMasterEnable();//!TODO - only enable timer interrupt for scheduler
	}
}  

//********************************OS_Signal************************************
//	Release a counting semaphore.
//! \param *thread Pointer to the thread that is calling this fucntion
//! \param semaPt Pointer to the semaphore to release
//! \return None
//*****************************************************************************
void OS_Signal(OS_TCB *thread, Sema4Type *semaPt){
	OS_TCB *tWaiting = NULL;
	IntMasterDisable();										//Ensure that PendSV does not interrupt while moving threads
	if(TCBList[BLOCKED][0] != NULL){						//from blocked list. Also make sure that "woken up" threads
															//aren't called until the semaphore is freed
		tWaiting = TCBList[BLOCKED][0];
		
		if(tWaiting->sema4BlockPt == semaPt){				//Check to see if head of list is waiting on the semaphore
			OS_SetPriority(tWaiting, tWaiting->oldPriority);//if it is, remove it from the blocked list
		}else{
			do{												//Look at the rest of the loop to see if any of the threads
				tWaiting = tWaiting->prev;				  	//are blocking on this semaphore. If one of them is, wake it
				if(tWaiting->sema4BlockPt == semaPt){		//up. ONLY WAKE UP ONE THREAD!		
					OS_SetPriority(tWaiting, tWaiting->oldPriority);
					break;
				}		
			}while(tWaiting != TCBList[BLOCKED][0]);
		}
	}
	semaPt->lastOwner = thread->id;
	semaPt->ownerID = -1;
	semaPt->Value--;										//Free the semaphore
	
	IntMasterEnable();
	
	if(tWaiting->priority <= thread->priority)
		OS_Suspend();	
} 

//*********************************OS_bWait************************************
//	Spin / blocks on a binary semaphore
//! \param *thread Pointer to the thread that is calling this fucntion
//! \param semaPt Pointer to the semaphore to wain on.
//! \return None
//*****************************************************************************
void OS_bWait(OS_TCB *thread, Sema4Type *semaPt){
	while(1){
		IntMasterDisable();
	   	if(semaPt->Value == 0){
			semaPt->ownerID = thread->id;
			semaPt->lastOwner = -1;
			semaPt->Value = 1;
			IntMasterEnable();
			break;
		}else{
			thread->oldPriority = thread->priority; //Preserve the original priority for
													//when the thread is "woken up"
			thread->sema4BlockPt = semaPt;			//If the semaphore is occupied, then
			OS_SetPriority(thread,BLOCKED);			//move the thread to the blocked list
			IntMasterEnable();						//and trigger a context switch
			OS_Suspend();
		}
		IntMasterEnable();//!TODO - only enable timer interrupt for scheduler
	}
}  

//*******************************OS_bSignal************************************
//	Releases a binary semaphore
//! \param *thread Pointer to the thread that is calling this fucntion
//! \param semaPt Pointer to the semaphore to release
//! \return None
//*****************************************************************************
void OS_bSignal(OS_TCB *thread, Sema4Type *semaPt){
	OS_TCB *tWaiting = NULL;
	IntMasterDisable();										//Ensure that PendSV does not interrupt while moving threads
	if(TCBList[BLOCKED][0] != NULL){						//from blocked list. Also make sure that "woken up" threads
															//aren't called until the semaphore is freed
		tWaiting = TCBList[BLOCKED][0];
		
		if(tWaiting->sema4BlockPt == semaPt){				//Check to see if head of list is waiting on the semaphore
			OS_SetPriority(tWaiting, tWaiting->oldPriority);//if it is, remove it from the blocked list
		}else{
			do{												//Look at the rest of the loop to see if any of the threads
				tWaiting = tWaiting->prev;				  	//are blocking on this semaphore. If one of them is, wake it
				if(tWaiting->sema4BlockPt == semaPt){		//up. ONLY WAKE UP ONE THREAD!		
					OS_SetPriority(tWaiting, tWaiting->oldPriority);
					break;
				}		
			}while(tWaiting != TCBList[BLOCKED][0]);
		}
	}
	semaPt->lastOwner = thread->id;
	semaPt->ownerID = -1;
	semaPt->Value = 0;										//Free the semaphore
	
	IntMasterEnable();
	
	if(tWaiting->priority <= thread->priority)
		OS_Suspend();
} 

//*************************os_decrement_sleep_counters*************************
//	Local function - should not be called outside this file
//	Loops through the OS threads and decrements the sleep couners by 1ms
//! \param current Current TCB
//! \return None
//	Note! Should be called exactly once every ms
//*****************************************************************************
static void os_decrement_sleep_counters(){
	int r;
	OS_TCB *tcbCur;

	for(r = 0; r < MAX_NUM_PRIORITIES; r++){
		tcbCur = TCBList[r][0];
				
		if(tcbCur == NULL)
			continue;
			
		do{
		   if(tcbCur->sleep_ctr > 0)
		   		tcbCur->sleep_ctr--;
		   
		   tcbCur = tcbCur->next;

		}while(tcbCur != TCBList[r][0]);
	}
}

//***************************os_get_next_ready_thread**************************
//	Local function - should not be called outside this file
//	Loops through the OS threads to find the next thread ready to execute. The
//	next thread ready to execute is defined as the highest priority thread 
//	with 0 sleep time. Note that the lowest priority is reserved for blocked
//	threads.
//! \return None
//*****************************************************************************
static OS_TCB* os_get_next_ready_thread(){
	int r, deadThreadPriority;
	OS_TCB *marker;
	OS_TCB *deadThread;

	//Check to see if the current thread should be deleted. If so, dellocate
	//it's stack space and remove it from the list
	if(OSTCBCur->deleted == true){
		deadThreadPriority = OSTCBCur->priority;
		deadThread = TCBList[deadThreadPriority][1]; 							//Same as OSTCBCur
		TCBList[deadThreadPriority][1] = TCBList[deadThreadPriority][1]->prev;	//Same as OSTCBCur->prev
		CDL_DELETE(TCBList[deadThreadPriority][0],deadThread); 					//TCBList[deadThreadPriority][0] is
																				//the head of priority list that contains OSTCBCur 
		free(deadThread);
		if(TCBList[deadThreadPriority][0] == NULL){				
			TCBList[deadThreadPriority][1] = NULL;				
		}										
	}

	//Loop through the priority lists looking for eligable threads. Start
	//with highest priority and iterate down to lowest priority. First non
	//sleeping thread is will be the new current thread. Priority lists are
	//executed in round robin fashion.
	for(r = 0; r < MAX_NUM_PRIORITIES; r++){
		if(TCBList[r][0] == NULL)
			continue;
		
		for(marker = TCBList[r][1], TCBList[r][1] = TCBList[r][1]->next;
		(TCBList[r][1] != marker && TCBList[r][1]->sleep_ctr != 0); 
		TCBList[r][1] = TCBList[r][1]->next);

		if(TCBList[r][1]->sleep_ctr == 0)
			return TCBList[r][1];//Should always return here unless there are no
								 //ready threads			
	}
	return NULL;//Should never get here - this is an error if it does
}


//********************************OSTaskSwHook*********************************
//	Decrements the sleep counters for each TCB. Finds the next thread ready to
//	run. This fucntion is called from PendSV
//! \param current Current TCB
//! \return None
//	Note! Should be called exactly once every ms
//*****************************************************************************
void OSTaskSwHook(void) {
	
	if  (OSTCBCur != NULL){
		
		OSTCBCur = os_get_next_ready_thread();
	}
}
 
//***********************************OS_Kill************************************
//	Removes a thread from the scheduler list and deallocates its memory. This is
//	called when a thread dies
//! \param current Current TCB
//! \return None
//*****************************************************************************
void OS_Kill(void){
	OSTCBCur->deleted = true;
	OS_Suspend();
	for(;;);	//Calls to this function should never return. PendSV interrupt
				//should switch context
}

//*******************************OS_ForceKill***********************************
//	Removes a thread from the scheduler list and deallocates its memory. This is
//	used to kill a third party thread
//! \param thread Pointer to thread that should be killed
//! \return None
//*****************************************************************************
void OS_ForceKill(OS_TCB* thread){
	thread->deleted = true;
	OS_Suspend();
	for(;;);  	//Calls to this function should never return. PendSV interrupt
				//should switch context
}

//****************************OS_SetPriority***********************************
//	Changes the priority of a thread.
//! \param *thread Pointer to the thead whose priority is being changed.
//! \param newPriority New priority level this thread should execute at
//! \return 1 on Success, 0 otherwise
//! WARNING! This function should only be called with interrupts disabled!
//*****************************************************************************
int OS_SetPriority(OS_TCB *thread, uint32_t newPriority){
	if(newPriority >=MAX_NUM_PRIORITIES+1)
		return 0;

	CDL_DELETE(TCBList[thread->priority][0],thread);	//Delete thread from current priority list
	
	if(TCBList[thread->priority][0] == NULL)			//If only thread in current priority list
		TCBList[thread->priority][1]=NULL;			  	//make sure current thread pointer for that
	else												//list is NULL
		TCBList[thread->priority][1] = thread->prev;   	//Make sure to update the current pointer
														//for the old priority list
															
	if(TCBList[newPriority][1] == NULL)					//If only thread in new priority list
		TCBList[newPriority][1] = thread;				//make sure current thread pointer is NOT NULL
	
	thread->priority = newPriority;
	
	CDL_PREPEND(TCBList[newPriority][0], thread);	   	//Put thread in new priority list.

	

	return 1;
} 

//******************************OS_AddThread***********************************
//	Adds a thread to the scheduler linked list. Thread TCBs are initialized
//	here before being added to the list.
//! \param task Entry point for the thread
//!	\param stackSize TCB stack size for the thread. This is currently ignored.
//!	\param priority	The priority of the thread.
//! \return 1 on success, 0 otherwise
//!	TODO! Implement stacksize
//! WARNING! This function should only be called with interrupts disabled!
//*****************************************************************************
int OS_AddThread(void(*task)(OS_TCB *this), 
   			unsigned long stackSize, unsigned int priority){
	
	static int threadID = 0;
	

	OS_TCB *thread = malloc(sizeof(OS_TCB));

	if (thread == NULL) {
		return 0;
	}
	thread->id = threadID++;
	thread->sleep_ctr = 0;
    thread->deleted = false;
	thread->priority = priority;
	thread->SP = &(thread->stack[127]);

	*(thread->SP)    = (uint32_t)0x01000000L; /* xPSR                                          */ 
    *(--thread->SP)  = (uint32_t)task;        /* Entry Point                                   */ 
    *(--thread->SP)  = (uint32_t)OS_Kill; 	  /* R14 (LR)                                      */ 
    *(--thread->SP)  = (uint32_t)0x12121212L; /* R12                                           */ 
    *(--thread->SP)  = (uint32_t)0x03030303L; /* R3                                            */ 
    *(--thread->SP)  = (uint32_t)0x02020202L; /* R2                                            */ 
    *(--thread->SP)  = (uint32_t)0x01010101L; /* R1                                            */ 
    *(--thread->SP)  = (uint32_t)thread; 	  /* R0 : pointer to tcb                           */ 
                                     		  /* Remaining registers saved on process stack    */ 
    *(--thread->SP)  = (uint32_t)0x11111111L; /* R11                                           */ 
    *(--thread->SP)  = (uint32_t)0x10101010L; /* R10                                           */ 
    *(--thread->SP)  = (uint32_t)0x09090909L; /* R9                                            */ 
    *(--thread->SP)  = (uint32_t)0x08080808L; /* R8                                            */ 
    *(--thread->SP)  = (uint32_t)0x07070707L; /* R7                                            */ 
    *(--thread->SP)  = (uint32_t)0x06060606L; /* R6                                            */ 
    *(--thread->SP)  = (uint32_t)0x05050505L; /* R5                                            */ 
    *(--thread->SP)  = (uint32_t)0x04040404L; /* R4                                            */
	
	
	if (TCBList[priority][0] == NULL) {
		TCBList[priority][0] = thread;
		TCBList[priority][1] = thread;
	}

	CDL_PREPEND(TCBList[priority][0], thread);
  
   	return 1;
}

//***********************************OS_Id*************************************
//	Returns the thread ID for the currently running thread	
//! \param None
//! \return ID of currently running thread
//*****************************************************************************
unsigned long OS_Id(void){
	return OSTCBCur->id;
}

//***************************OS_AddPeriodicThread*******************************
//	Adds a background periodic task.
//! \param task Entry point for the thread
//!	\param period period given in ms
//!	\param priority	The priority of the thread. This field is currently ignored.
//! \return 1 on success, 0 otherwise
//!	TODO! Implement priority
//*****************************************************************************
void (*func_array[2])(void);  //function pointer array for use in the timers
int OS_AddPeriodicThread(void(*task)(void), unsigned long period, unsigned long priority) {
	static int count = 0;
	//if its the first time this function gets called
	if (count == 0) {
		//put function into array
		func_array[0] = task;
		//configure timer 1 to execute function inside its interrupt handler
		SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);
		TimerConfigure(TIMER1_BASE, TIMER_CFG_32_BIT_PER);
	    TimerLoadSet(TIMER1_BASE, TIMER_A, (period * (SysCtlClockGet()/2000)));
		IntPrioritySet(INT_TIMER1A, priority);
		IntEnable(INT_TIMER1A);
		TimerIntEnable(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
		TimerEnable(TIMER1_BASE, TIMER_A);
	}
	//if it is the second time
	else if (count == 1) {
		//put function into array
		func_array[1] = task;
		//configure timer 0 to execute funtion inside its interrupt handler
		SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER2);
		//32-bit periodic interrupt at /period ms
		TimerConfigure(TIMER2_BASE, TIMER_CFG_32_BIT_PER);
	    TimerLoadSet(TIMER2_BASE, TIMER_A, (period * (SysCtlClockGet()/2000)));
		IntPrioritySet(INT_TIMER2A, priority);
		IntEnable(INT_TIMER2A);
		TimerIntEnable(TIMER2_BASE, TIMER_TIMA_TIMEOUT);
		TimerEnable(TIMER2_BASE, TIMER_A);

	}
	else if (count > 1) {
	return 0; // error, only 2 preriodic tasks are allowed
	}
	count++;
	return 1;
}
//*****************************OS_AddButtonTask********************************
//	Adds a background thread to run whenever the up button is pressed on the
//	board.
//! \param task Entry point for the thread
//!	\param priority	The priority of the thread. This field is currently ignored.
//! \return 1 on success, 0 otherwise
//!	TODO! Implement priority
//*****************************************************************************
int OS_AddButtonTask(void(*task)(void), unsigned long priority){
	
	//
    // Enable the peripherals used by the application.
    //

	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);


	//
    // Configure the GPIOs used to read the state of the on-board push buttons.
    //
    GPIOPinTypeGPIOInput(GPIO_PORTE_BASE,
                         GPIO_PIN_1);

    GPIOPadConfigSet(GPIO_PORTE_BASE,
                     GPIO_PIN_1,
                     GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU);

	GPIOIntTypeSet(GPIO_PORTE_BASE,GPIO_PIN_1,GPIO_FALLING_EDGE);
	GPIOPinIntEnable(GPIO_PORTE_BASE, GPIO_PIN_1);
	IntPrioritySet(INT_GPIOE, priority);
	IntEnable(INT_GPIOE);
	return 1;
}

int OS_AddButtonSelectTask(void(*task)(void), unsigned long priority) {

			
	//
    // Enable the peripherals used by the application.
    //

	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);


	//
    // Configure the GPIOs used to read the state of the on-board push buttons.
    //
    GPIOPinTypeGPIOInput(GPIO_PORTF_BASE,
                         GPIO_PIN_1);

    GPIOPadConfigSet(GPIO_PORTF_BASE,
                     GPIO_PIN_1,
                     GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU);

	GPIOIntTypeSet(GPIO_PORTF_BASE,GPIO_PIN_1,GPIO_FALLING_EDGE);
	GPIOPinIntEnable(GPIO_PORTF_BASE, GPIO_PIN_1);
	IntPrioritySet(INT_GPIOF, priority);
	IntEnable(INT_GPIOF);
	return 1;




}

//*********************************OS_Suspend**********************************
//	Suspends the current thread and triggers a context switch
//! \param None
//! \return None
//	NOTE! Be careful! PendSV may not trigger immediately! Upto 12 clock cycles
//	can pass before PendSV is triggered
//*****************************************************************************
void OS_Suspend(void){
	OSCtxSw();
}

//**********************************OS_Sleep***********************************
//	Suspends the current thread for the given number of ms. Context switch 
//	is triggered
//! \param *thread Pointer to the thread that is calling this fucntion
//! \return None
//	NOTE! Be careful! PendSV may not trigger immediately! Upto 12 clock cycles
//	can pass before PendSV is triggered
//*****************************************************************************
void OS_Sleep(OS_TCB* thread, unsigned long sleepTime){
	thread->sleep_ctr = sleepTime;
	OS_Suspend();	
} 

//*******************************OS_MailBox_Init*******************************
//	Initialize mailbox communication channel. Data cannot be placed in a mail-
//	box until it has been read.
//! \param None
//	\return None
//*****************************************************************************
void OS_MailBox_Init(void){
	static bool_t initialized = false;
	
	if(initialized == true){		
		return;
	}else{
	 	initialized = false;
	}

	OS_InitSemaphore(&BoxFree,0,1);
	OS_InitSemaphore(&DataValid,0,1);
}
//*******************************OS_MailBox_Send*******************************
//	Put data in the mailbox to be read by another thread
//!	\param *thread Pointer to the thread that is calling this fucntion
//! \param data	4 bytes of data
//	\return None
//	NOTE! This fucntion will spin/block if the MailBox contains data not yet
//	received
//*****************************************************************************
void OS_MailBox_Send(OS_TCB *thread, unsigned long data){
	OS_bWait(thread,&BoxFree);
	MailBoxData = data;
	OS_bSignal(thread,&DataValid);		
}
//*******************************OS_MailBox_Recv*******************************
//	Recieve data from the mailbox
//! \param *thread Pointer to the thread that is calling this fucntion
//	\return Data recieved from the mailbox
//	NOTE! This fucntion will spin/block if the MailBox is empty
//*****************************************************************************
unsigned long OS_MailBox_Recv(OS_TCB *thread){
	unsigned long data;
	OS_bWait(thread,&DataValid);
	data = MailBoxData;
	OS_bSignal(thread,&BoxFree);
	return data;
}


//***********************************OS_Time***********************************
//	Returns the time in 20ns units
//! \param None
//	\return System time in 20ns units
//	NOTE! System time can roll over
//*****************************************************************************
unsigned long long OS_Time(void){
	return OSTime + (50000L - NVIC_ST_CURRENT_R);
}

//***********************************OS_Time***********************************
//	Returns the time in 1ms units
//! \param None
//	\return System time in 20ns units
//	NOTE! System time can roll over
//*****************************************************************************
unsigned long long OS_MSTime(void){
	return Global_timerMs = 0;;
}


//*******************************OS_TimeDifference*****************************
//	Calculates difference between two times
//! \param stop End time
//!	\param start Start time
//	\return Time difference in 20ns units
//*****************************************************************************
long long OS_TimeDifference(unsigned long long stop, unsigned long long start){

	return (stop - start);

}
//********************************OS_ClearMsTime*******************************
//	Sets the system time to 0
//! \param None
//	\return None
//*****************************************************************************
void OS_ClearMsTime(void){
 	Global_timerMs = 0;	
}

//**********************************OS_Launch**********************************
//	Launches the operating system. OS_Init() should be called prior to this
//! \param theTimeSlice Time slice for each thread. Not currently implemented
//	\return None
//!	TODO! Implement thread time slices
//*****************************************************************************
void OS_Launch(unsigned long theTimeSlice){
	OSTCBCur = os_get_next_ready_thread();
	OSStartHighRdy();
}

//*****************************************************************************
//
// The interrupt handler for the for Systick interrupt.
//
//*****************************************************************************
void SysTickIntHandler(void) {
	OSTime = OSTime + 50000;
	Global_timerMs++;
	os_decrement_sleep_counters();
	OSCtxSw();
}
